import numpy as np

'''
X，Y下，Z左
Mx前，My上，Mz右
'''
'''
天平坐标系→机体坐标系
天平轴: X向后为正, Y向下为正, Z向左为左
机体轴: X向前为正, Y向右为正, Z向下为正(美式)
气流轴：(美式)
'''
def transform_a2b(input):
    input = np.array(input)
    output = np.ones_like(input)

    output[:,0] = -input[:,0] # X轴向力
    output[:,1] = -input[:,2] # Y横向力
    output[:,2] = input[:,1] # Z法向力

    output[:,3] = input[:,3] # L滚转力矩
    output[:,4] = input[:,5] # M俯仰力矩
    output[:,5] = -input[:,4] # N偏航力矩

    output[:,-1] = input[:,-1] # 速度

    return output


'''
气动力坐标转换（机体坐标→气流坐标）
INPUT: 机体轴气动力, 运动角度
OUTPUT: 气流坐标系
* 气流坐标系和地坐标系不重合！
'''
def transform_b2w(f_body, angle):
    f_body = np.array(f_body)
    angle = np.array(angle)
    f_wind = np.zeros_like(f_body)

    Alpha = np.zeros((len(angle),1))
    Beta = np.zeros((len(angle),1))

    Angle_wind = np.zeros((len(angle),2))

    for i in range(f_body.shape[0]):

        # 滚转角和偏航角对换？
        phi = angle[i,0]/180*np.pi  # 滚转角
        theta = angle[i,1]/180*np.pi    # 俯仰角
        psi = angle[i,2]/180*np.pi  # 偏航角

        # 计算气流角（王家骏）
        '''
        地轴速度: [vx,vy,vz] = [-30,0,0]
        地转体: R_g2b
        体轴速度: [u v w] = R_g2b @ [vx,vy,vz]
        攻角定义: alpha = arctan(w/u)
        侧滑角定义: beta = arcsin(V/) 
        '''
        
        alpha= np.arctan(np.tan(theta)*np.cos(phi)+np.sin (phi)*np.tan(psi)/ np.cos(theta))
        beta = np.arcsin(np.sin(theta)*np.sin(phi)*np.cos (psi)-np.sin (psi)*np.cos(phi))

        Alpha[i] = alpha/np.pi*180
        Beta[i] = beta/np.pi*180

        # 机体轴转气流轴
        R_b2w = [[np.cos(beta)*np.cos(alpha),np.sin(beta),np.cos(beta)*np.sin(alpha)],
                 [-np.sin(beta)*np.cos(alpha),np.cos(beta),-np.sin(beta)*np.sin(alpha)],
                 [-np.sin(alpha),0,np.cos(alpha)]]


        f_wind[i,0:3] = (R_b2w @ f_body[i,0:3].reshape((3,1))).flatten()
        f_wind[i,3:6] = f_body[i,3:6]
        f_wind[i,-1]  = f_body[i,-1]


    Angle_wind[:,0] = Alpha.flatten()
    Angle_wind[:,1] = Beta.flatten()

    return f_wind.tolist(), Angle_wind



'''
气动力坐标转换（气流坐标→常用）
和研究所结果
'''
def transform_w2r(input):
    input = np.array(input)
    output = np.ones_like(input)

    output[:,0] = -input[:,0] # 阻力
    output[:,1] = -input[:,1] # 侧立
    output[:,2] = -input[:,2] # 升力

    output[:,3] = input[:,3] # L滚转力矩
    output[:,4] = -input[:,4] # M俯仰力矩
    output[:,5] = input[:,5] # N偏航力矩

    output[:,-1] = input[:,-1] # 速度

    return output
